package es.uned.dia.james;

public class SFController {	    
    private LinearPlant plant;
    // feedback gains
    private double[] K;
    // feedback thresholds
    private double alpha, lambda;
    // feedforward gains
    private double[] Kff;
    // feedforward thresholds
    private double[] deltaFF;
    private double usf, uff;
    // set point
//    private double ysp, yspPrev, esp;
    private double[] sp, spPrev;
    private double[] ePrev;
	private double tPrev;
    
    private int order;
    // FSM: event, states and transitions
    private int state;
    
    public SFController() {
    	init();
    }
    
    public void init() { 
    	plant = null;
    	K = Kff = null;
    	deltaFF = null;
    	ePrev = null;
    	spPrev = null;
    	sp = null;
    	usf = uff = 0;
//        ysp = yspPrev = esp = 0;
        tPrev = 0;  
        order = 1;
    	state = order;    	
    }
    
    
    public void setThresholds(double alpha, double lambda) {
        setAlpha(alpha);
        setLambda(lambda);
    }
    
    public void setDeltaFF(double[] deltaFF) { if(deltaFF != null && deltaFF.length == order) for(int i=0; i<order; i++) this.deltaFF[i] = deltaFF[i]; }

    public void setKff(double[] Kff) { if(Kff != null && Kff.length == (order+1)) for(int i=0; i<=order; i++) this.Kff[i] = Kff[i]; }
    
    public void setGains(double[] K) {
    	if(K.length != order) return;    	
        this.K = K.clone();
    }
    
    public void setAlpha(double alpha) { this.alpha = alpha; }
    
    public void setLambda(double lambda) { this.lambda = lambda; }
    
    public boolean connect(LinearPlant plant) {
        if(plant == null) return false;
        
        this.plant = plant; 
        order = plant.states;
        state = order;
        Kff = new double[order+1];
        deltaFF = new double[order];
        ePrev = new double[order];
        sp = new double[order];
        spPrev = new double[order];        
        return true;
    }

   
/*    public void event(FeedforwardEvent ev) {     	
        state = (state + 1) % (order + 1);
        uff = Kff[state];
    	System.out.println("FF event");
    	System.out.println("  state = " + state);
    	System.out.println("  Kff = " + Kff[state]);
    	if(state != order) System.out.println("  deltaFF = " + deltaFF[state]);
    }
  */
    
    public void event(SetpointEvent ev) {
    	tPrev = ev.t;
    	ePrev = ev.x.clone();
    	spPrev = sp.clone();
    	sp = ev.sp.clone();
    	state = 0;
        uff = 0.28*ev.sp[0]; //Kff[state];
        for(int i=0; i<order; i++) { ePrev[i] = sp[i] - ev.x[i]; usf += K[i]*(sp[i] - ev.x[i]); }
    }
    
    public void event(EventV ev) {
    	if(state++ >= order) state = order;
    	
    	tPrev = ev.t;
        usf = 0;
        for(int i=0; i<order; i++) { ePrev[i] = sp[i] - ev.x[i]; usf += K[i]*(sp[i] - ev.x[i]); }
    		
System.out.println("!!!!!!!!!!!!!!!!!!!!!!!!!!!");    		
    	System.out.println("Event");
    	System.out.println("  state = " + state);
    	System.out.println("  uff = " + uff);
    	System.out.println("  usf = " + usf);
    }
    
    public double evaluateEvent(EventV ev) {
    	if(state >= order) {
        	double e = 0; for(int i=0; i<ePrev.length; i++) e += Math.pow((sp[i] - ev.x[i]) - ePrev[i], 2);
        	return Math.sqrt(e) - alpha*Math.exp(-lambda*(ev.t-tPrev));
//        	return 1.0;
    	} else
    		return (sp[0] - ev.x[0]) - deltaFF[state]*(sp[0]-spPrev[0]);
    }

/*    public double evaluate(EventInfo ei) {
    	if(state < 0 || state >= order) 
    		return (ei.ysp == this.ysp) ? 1.0 : - 1.0;
    	else
    		return ei.y - deltaFF[state];
    	
    }
*/  
/*    public double evaluateFeedforwardEvent(double y) {
    	if(state < 0 || state >= order) 
    		return 1.0;
    	else
    		return y - deltaFF[state];
    }
*/
    public double u() {    	
        return uff + usf;
    }
       
/*    public void setPoint(double t, double e, double ysp) {
    	if(state != order) return;
    	
//        event[0] = true;
//        event(new FeedforwardEvent(t, e));
        this.esp = ysp - this.ysp;
        this.ysp = ysp;
    }
*/    
}    